﻿#include "parsedata.h"
#include <sys/time.h>
#include <unistd.h>
#include <fstream>
#include<iomanip>
#include<ctime>

using namespace std;

GPS_INI gps_ini;
Vehiclestatus vehicle_status;
imu IMU;
daoyuan_imu DaoYuan_imu;
CPT CPT_imu;

double my_yaw = 0;
INS_D ins_d;
ParseData::ParseData()
{
	LCM_ = new lcm::LCM("udpm://239.255.76.67:7667?ttl=1");
	m_bSysTime = false;
	SysTime();
	m_lCurTime=0;

}
void ParseData::SysTime()
{
	m_bSysTime = true;
}

//....split String......//
void ParseData::SplitString(const string &s, vector<string> &v,
							const string &c)
{
	string::size_type pos1, pos2;
	pos2 = s.find(c);
	pos1 = 0;
	while (string::npos != pos2)
	{
		v.push_back(s.substr(pos1, pos2 - pos1));

		pos1 = pos2 + c.size();
		pos2 = s.find(c, pos1);
	}
	if (pos1 != s.length())
		v.push_back(s.substr(pos1));
}
std::vector<u_int8_t> mm_strRecv;
bool ParseData::gpsParse(std::vector<u_int8_t> strRecv)
{
	mm_strRecv.insert(mm_strRecv.end(), strRecv.begin(), strRecv.end());
	uint i, j;
	string str, strNEMA;

	//先判断是否接收到数据
	if (mm_strRecv.empty())
		return false;
	//若字符串不是以'$'开头的,必须删掉这部分不完整的
	if (mm_strRecv[0] != '$')
	{
		vector<u_int8_t>::iterator ite1 =
			find(mm_strRecv.begin(), mm_strRecv.end(), '$');
		i = std::distance(mm_strRecv.begin(), ite1);

		// i = mm_strRecv.indexOf('$', 0);

		if (i > 0)
		{
			mm_strRecv.erase(mm_strRecv.begin(), ite1);
		}
	}
	if (mm_strRecv.empty())
		return false;
	//截取完整的NMEA-0183输出语句(m_strRecv中可能有多条语句,每条间以/r/n分隔)
	for (;;)
	{
		vector<u_int8_t>::iterator ite1 =
			find(mm_strRecv.begin(), mm_strRecv.end(), '\n');
		i = std::distance(mm_strRecv.begin(), ite1);
		// i = mm_strRecv.indexOf('\n', 0);
		if (ite1 == mm_strRecv.end())
			break; //所有的完整输出语句都已经处理完毕,退出循环

		//截取完整的NMEA-0183输出语句
		strNEMA.assign(mm_strRecv.begin(), ite1);
		mm_strRecv.erase(mm_strRecv.begin(), ite1);

		//下面对各种输出语句进行分别处理

		if (strNEMA.find("$GPRMC", 0) == 0 || strNEMA.find("$GNRMC", 0) == 0)
		{
			//该输出语句中的各项以','分隔
			vector<string> gpsByteArrays;
			SplitString(strNEMA, gpsByteArrays, ",");

			if (gpsByteArrays.at(7) != "")
			{
				gps_ini.Speed = atof(gpsByteArrays.at(7).c_str()) * 1.852;
			}
			if (gpsByteArrays.at(8) != "")
			{
				gps_ini.Heading = atof(gpsByteArrays.at(8).c_str());
				if (gps_ini.Heading <= 270)
				{
					gps_ini.Heading = 90 - gps_ini.Heading;
				}
				else if (gps_ini.Heading > 270)
				{
					gps_ini.Heading = 360 + 90 - gps_ini.Heading;
				}
			}
			//cout <<"gps:"<<gpsByteArrays.size()<<endl;
			struct timeval tv;
			if (gpsByteArrays.at(1) != "" && gpsByteArrays.at(9) != "" && m_bSysTime)
			{
				int32_t hour = std::atoi(gpsByteArrays.at(1).substr(0, 2).c_str());
				int32_t minite = std::atoi(gpsByteArrays.at(1).substr(2, 2).c_str());
				int32_t sec = std::atoi(gpsByteArrays.at(1).substr(4, 2).c_str());
				int32_t microsec = std::atoi(gpsByteArrays.at(1).substr(7, 3).c_str());

				int day = std::atoi(gpsByteArrays.at(9).substr(0, 2).c_str());
				int month = std::atoi(gpsByteArrays.at(9).substr(2, 2).c_str());
				int year = std::atoi(("20" + gpsByteArrays.at(9).substr(4, 2)).c_str());

				uint32_t ret;
				const uint32_t mon_yday[2][12] = {
					{0, 31, 59, 90, 120, 151, 181, 212, 243, 273, 304, 334},
					{0, 31, 60, 91, 121, 152, 182, 213, 244, 274, 305, 335},
				};
				int i = 0;
				// 以平年时间计算的秒数
				ret = (year - 1970) * 365 * 24 * 3600;
				ret += (mon_yday[(year) % 4 == 0 && ((year) % 100 != 0 || (year) % 400 == 0)][month] + day - 1) * 24 * 3600;
				ret += hour * 3600 + minite * 60 + sec;
				// 加上闰年的秒数
				for (i = 1970; i < year; i++)
				{
					if (((i) % 4 == 0 && ((i) % 100 != 0 || (i) % 400 == 0)))
					{
						ret += 24 * 3600;
					}
				}
				if (ret > 4107715199)
				{ // 2100-02-29 23:59:59
					ret += 24 * 3600;
				}
				tv.tv_sec = ret;
				tv.tv_usec = microsec * 1000;
				settimeofday(&tv, NULL);
				m_bSysTime = false;
				cout<< "Time has sys"<<endl;
			}
		}
		else if (strNEMA.find("$GPGGA", 0) == 0 ||
				 strNEMA.find("$GNGGA", 0) == 0)
		{
			gps_ini.strGGA = strNEMA;
			vector<string> gpsByteArrays;
			SplitString(strNEMA, gpsByteArrays, ",");
			if (gpsByteArrays.size() > 2 && gpsByteArrays.at(2) != "")
			{
				double lat = atof(gpsByteArrays.at(2).c_str()) / 100;
				int ilat = (int)floor(lat) % 100;
				lat = ilat + (lat - ilat) * 100 / 60; //纬度
				if (lat > 10 && lat < 90)
				{
					gps_ini.Lat = lat;
				}
				double lon = atof(gpsByteArrays.at(4).c_str()) / 100;
				int ilon = (int)floor(lon) % 1000;
				lon = ilon + (lon - ilon) * 100 / 60; //经度
				if (lon < 180 && lon > 20)
				{
					gps_ini.Lon = lon;
					gps_ini.high = atoi(gpsByteArrays.at(5).c_str());
				}
				gps_ini.statu = atoi(gpsByteArrays.at(6).c_str());
				gps_ini.HDOP = atof(gpsByteArrays.at(8).c_str());
				gps_ini.StarNum = atoi(gpsByteArrays.at(7).c_str());
				// cout<<gps_ini.Lon <<"  "<<gps_ini.Lat;
				struct timeval tv;
				if (gpsByteArrays.at(1) != ""  && m_bSysTime)
				{
					int32_t hour = std::atoi(gpsByteArrays.at(1).substr(0, 2).c_str());
					int32_t minite = std::atoi(gpsByteArrays.at(1).substr(2, 2).c_str());
					int32_t sec = std::atoi(gpsByteArrays.at(1).substr(4, 2).c_str());
					int32_t microsec = std::atoi(gpsByteArrays.at(1).substr(7, 3).c_str());

					//获取系统时间戳
					time_t timeReal;
					time(&timeReal);
					timeReal = timeReal + 8*3600;
					tm* t = gmtime(&timeReal); 

					int day = t->tm_mday;
					int month = t->tm_mon;
					int year = t->tm_year + 1900;

					uint32_t ret;
					const uint32_t mon_yday[2][12] = {
						{0, 31, 59, 90, 120, 151, 181, 212, 243, 273, 304, 334},
						{0, 31, 60, 91, 121, 152, 182, 213, 244, 274, 305, 335},
					};
					int i = 0;
					// 以平年时间计算的秒数
					ret = (year - 1970) * 365 * 24 * 3600;
					ret += (mon_yday[(year) % 4 == 0 && ((year) % 100 != 0 || (year) % 400 == 0)][month] + day - 1) * 24 * 3600;
					ret += hour * 3600 + minite * 60 + sec;
					// 加上闰年的秒数
					for (i = 1970; i < year; i++)
					{
						if (((i) % 4 == 0 && ((i) % 100 != 0 || (i) % 400 == 0)))
						{
							ret += 24 * 3600;
						}
					}
					if (ret > 4107715199)
					{ // 2100-02-29 23:59:59
						ret += 24 * 3600;
					}
					tv.tv_sec = ret;
					tv.tv_usec = microsec * 1000;
					settimeofday(&tv, NULL);
					m_bSysTime = false;
					cout<< "Time has sys"<<endl;
				}
			}
			else
			{
				gps_ini.statu = 0;
				gps_ini.HDOP = 0;
				gps_ini.StarNum = 0;
			}
		}
		else if (strNEMA.find("$PTNL", 0) == 0)
		{
			vector<string> gpsByteArrays;
			SplitString(strNEMA, gpsByteArrays, ",");
			// gps_ini.
			if (gpsByteArrays.size() >= 6 && gpsByteArrays.at(3) != "0")
			{
				if (gpsByteArrays.at(3) != "")
				{
					gps_ini.lat_ini = gps_ini.Lat;
					gps_ini.lon_ini = gps_ini.Lon;
					double raw_heading = atof(gpsByteArrays.at(3).c_str()) - 0.3670971264;
					if (raw_heading < 0)
					{
						raw_heading = 360 + raw_heading;
					}
					double yaw = 180 - raw_heading;
					gps_ini.yaw = yaw * M_PI / 180;
				}
				if (gpsByteArrays.at(5) != "")
				{
					gps_ini.pitch = atof(gpsByteArrays.at(5).c_str());
				}
				if (gpsByteArrays.at(7) != "")
				{
					gps_ini.roll = atof(gpsByteArrays.at(7).c_str());
				}
				if (gpsByteArrays.at(10) != "")
				{
					gps_ini.AltitudeStu = atoi(gpsByteArrays.at(10).c_str());
				}
			}
		}
		else if (strNEMA.find("$GPFPD", 0) == 0)
		{
			vector<string> gpsByteArrays;
			SplitString(strNEMA, gpsByteArrays, ",");

			if (gpsByteArrays.at(6) != "")
			{
				double pitch = 0.0;
				double roll = 0.0;
				double vehE = 0.0;
				double vehN = 0.0;
				gps_ini.Lat = atof(gpsByteArrays.at(6).c_str());
				gps_ini.Lon = atof(gpsByteArrays.at(7).c_str());
				gps_ini.Heading = atof(gpsByteArrays.at(3).c_str());
				if (gps_ini.Heading <= 270)
				{
					gps_ini.Heading = 90 - gps_ini.Heading;
				}
				else if (gps_ini.Heading > 270)
				{
					gps_ini.Heading = 360 + 90 - gps_ini.Heading;
				}
				vehE = atof(gpsByteArrays.at(9).c_str());
				vehN = atof(gpsByteArrays.at(10).c_str());
				pitch = atof(gpsByteArrays.at(4).c_str());
				roll = atof(gpsByteArrays.at(5).c_str());
				gps_ini.Speed == sqrt(vehN * vehN + vehE * vehE) * 3.6;
			}
		}
		else
			return false; //格式错误
	}
	return true;
}
void ParseData::handleMessage(const lcm::ReceiveBuffer *rbuf,
							  const std::string &chan,
							  const lcmtypes::vehicle_status_t *msg)
{
	vehicle_status.DisL = msg->disLeft;
	vehicle_status.DisR = msg->disRight;
	vehicle_status.speL = msg->speLeft;
	vehicle_status.speR = msg->speRight;
	vehicle_status.steerangle = msg->steerPos;
	vehicle_status.yawrate = msg->yawRate;
	vehicle_status.acclat = msg->accelerationLat;
	vehicle_status.acclon = msg->accelerationLon;
	// qDebug()<<"DR:"<<vehicle_status.DisL<<"DL:"<<vehicle_status.DisR;
}

void ParseData::ggaMessage(const lcm::ReceiveBuffer *rbuf,
						   const std::string &chan,
						   const lcmtypes::buffer_data_t *msg)
{
	int d = msg->data_length;
	std::vector<u_int8_t> qa;
	qa.resize(d);
	memcpy(qa.data(), &(msg->data[0]), d); // sizeof(uint8_t)*msg->numRanges);
	// msg->ranges;
	gpsParse(qa);
	parse_data(qa);
	// parse_imudata(qa);
}

void ParseData::imuMessage(const lcm::ReceiveBuffer *rbuf,
						   const std::string &chan,
						   const lcmtypes::buffer_data_t *msg)
{
	int d = msg->data_length;
	std::vector<u_int8_t> qa;
	qa.resize(d);
	memcpy(qa.data(), &(msg->data[0]), d); // sizeof(uint8_t)*msg->numRanges);
										   // msg->ranges;
										   // parse_imudata(qa);
										   // parse_imu3_data(qa);
}
void ParseData::gnssMessage(const lcm::ReceiveBuffer *rbuf,
							const std::string &chan,
							const lcmtypes::buffer_data_t *msg)
{
 
	if (gps_ini.ins_type == "daoyuan")
	{
		int d = msg->data_length;
		std::vector<u_int8_t> qa;
		qa.resize(d);
		memcpy(qa.data(), &(msg->data[0]), d); // sizeof(uint8_t)*msg->numRanges);
		// msg->ranges;
		parse_daoyuan_data(qa);
	}
	else if (gps_ini.ins_type == "cpt")
	{
		// time_t now = time(0);
		// tm *ltm = localtime(&now);
		// string rtime = 
		// 							to_string(1900+ltm->tm_year)+"_"+
		// 							to_string(1+ltm->tm_mon)+"_"+
		// 							to_string(ltm->tm_mday)+"_"+
		// 							to_string(ltm->tm_hour)+"_"+
		// 							to_string(ltm->tm_min);
		// str1 = "imu_data_"+rtime+".txt";
		int d = msg->data_length;
		std::vector<u_int8_t> qa;
		qa.resize(d);
		memcpy(qa.data(), &(msg->data[0]), d); // sizeof(uint8_t)*msg->numRanges);
		// msg->ranges;
		cout<<"cpt";//repeat print
		parse_data(qa);
	}
	else if (gps_ini.ins_type == "7660")
	{
		int d = msg->data_length;
		std::vector<u_int8_t> qa;
		qa.resize(d);
		memcpy(qa.data(), &(msg->data[0]), d); // sizeof(uint8_t)*msg->numRanges);
											   // msg->ranges;
											   // XWgpsParse(qa);
	}
	else if (gps_ini.ins_type == "ins_d")
	{
		int d = msg->data_length;
		std::vector<u_int8_t> qa;
		qa.resize(d);
		memcpy(qa.data(), &(msg->data[0]), d); // sizeof(uint8_t)*msg->numRanges);
		parse_ins_d_data(qa);
	}
}
std::vector<u_int8_t> imu_strRecv;
union cArray2intArray
{
	int16_t i[5];
	uint8_t c[10];
} c2i;
union cArray2floatArray
{
	float i[19];
	uint8_t c[76];
} c2f;

const  char * const cptstatus[] =  {
		"INS_INACTIVE" ,
		"INS_ALIGNING" ,
		"INS_HIGH_VARIANCE",
		"INS_SOLUTION_GOOD" ,
		"INS_SOLUTION_FREE" ,
		"INS_ALIGNMENT_COMPLETE" ,
		"DETERMINING_ORIENTATION" ,
		"WAITING_INITIALPOS" } ;
long fronttime=0;
bool ParseData::ljlparse_data(std::vector<u_int8_t> &imu_strRecv,ofstream& ofs)
{
	while (imu_strRecv.size() >=104) //至少包含帧头（2字节）、长度（1字节）、功能位（1字节）；根据设计不同而不同
	{
		// 2.1 查找数据头
		if (imu_strRecv[0] == 0xAA &&imu_strRecv[1] == 0x44&&imu_strRecv[2] == 0x13) //传输数据有帧头，用于判断. 找到帧头  AA AA 0A
		{
			ushort ID = 0;
			memcpy(&ID, imu_strRecv.data() + 4, 2);
			short len = 0;
			ulong weekseconds= 604800000;//every week milseconds
			ulong devseconds = 315964800000;//from   1980-1-6 00:00 begin time 
			ulong imu_time = (int)(imu_strRecv[8])+(int)(imu_strRecv[9]<<8)+(int)(imu_strRecv[10]<<16)+(int)(imu_strRecv[11]<<24);
			CPT_imu.utime =*((ushort*)&imu_strRecv[6]) * weekseconds + imu_time+devseconds;
			if (ID == 508)
			{
				unsigned char length = (unsigned char)imu_strRecv[3];
				len = (short)length + 16; 
				memcpy(&CPT_imu, imu_strRecv.data() + 12, (int)length);//ljl  头部长度是12
				ofs<<  CPT_imu.utime<<" ";	
				ofs << CPT_imu.azimuth<<" ";  //endl用于换行
				ofs << CPT_imu.roll<<" ";  //endl用于换行
				ofs << CPT_imu.pitch <<" ";  //endl用于换行
				ofs << CPT_imu.latitude<<" " ;  //endl用于换行											
				ofs << CPT_imu.longitude<<" ";  //endl用于换行
				ofs << CPT_imu.altitude<<" ";  //endl用于换行
				ofs << CPT_imu.northVelocity<<" ";  //endl用于换行
				ofs << CPT_imu.eastVelocity<<" ";  //endl用于换行
				ofs << CPT_imu.upVelocity<< endl;  //endl用于换行
				imu_strRecv.erase(imu_strRecv.begin(), imu_strRecv.begin() + 104);
				if(CPT_imu.utime-fronttime!=20)
				{
					cout<<"parse time is wrong"<<endl;
					cout<<"CPT_imu.utime"<<CPT_imu.utime<<endl;
					cout<<"time cost is "<<CPT_imu.utime-fronttime<<endl;
				}
				fronttime = CPT_imu.utime;

		    }
			else if (ID == 1465)
			{
				//cout<<"1465"<<endl;
				len = (imu_strRecv[9] << 8) + imu_strRecv[8] + 32;
				if (imu_strRecv.size() < len) //数据区尚未接收完整，
				{
					break; //跳出接收函数后之后继续接收数据
				}
				float azimuthdev = 0;
				memcpy(&azimuthdev, imu_strRecv.data() + 28 + 116, 4);
				imu_strRecv.erase(imu_strRecv.begin(), imu_strRecv.begin() + len);
				CPT_imu.Z_Accuracy = azimuthdev;
                //ofs<<"i will write the imu data"<<endl;
                cout<<"1465 CPT_imu.Z_Accuracy is "<<(float)CPT_imu.Z_Accuracy<<endl;
			}
		}else {
            	//帧头长度不正确时，清除
				imu_strRecv.erase(imu_strRecv.begin(), imu_strRecv.begin()+1); //为啥begin()+1,这个1的实际长度多少
		}
		
	}
}

bool ParseData::ljlparse_data(std::vector<u_int8_t> &imu_strRecv,list<CPT> &cptListBuffer)
{
	while (imu_strRecv.size() >=104) //至少包含帧头（2字节）、长度（1字节）、功能位（1字节）；根据设计不同而不同
	{
		// 2.1 查找数据头
		if (imu_strRecv[0] == 0xAA &&imu_strRecv[1] == 0x44&&imu_strRecv[2] == 0x13) //传输数据有帧头，用于判断. 找到帧头  AA AA 0A
		{

			ushort ID = 0;
			memcpy(&ID, imu_strRecv.data() + 4, 2);
			short len = 0;
			ulong weekseconds= 604800000;//every week milseconds
			ulong devseconds = 315964800000;//from   1980-1-6 00:00 begin time 
			ulong imu_time = (int)(imu_strRecv[8])+(int)(imu_strRecv[9]<<8)+(int)(imu_strRecv[10]<<16)+(int)(imu_strRecv[11]<<24);
			CPT_imu.utime =*((ushort*)&imu_strRecv[6]) * weekseconds + imu_time+devseconds;
			if (ID == 508)
			{
				imu_lasttime = imu_time;
                unsigned char length = (unsigned char)imu_strRecv[3];
                len = (short)length + 16;
                if (imu_strRecv.size() < len) //数据区尚未接收完整，
                {//跳出接收函数后之后继续接收数据
                	break; 
                }
                //执行其他代码，对数据进行处理。
                //解析5 6， 7 8字节的经纬度.
                uint32_t week;
                memcpy(&CPT_imu, imu_strRecv.data() + 12, (int)length);//ljl  头部长度是12
                imu_strRecv.erase(imu_strRecv.begin(), imu_strRecv.begin() + len);
				CPT mcpt;
					mcpt.utime = CPT_imu.utime;	//@ljl 写入文件的速度比读取要慢
					mcpt.azimuth = CPT_imu.azimuth;  //endl用于换行
					mcpt.roll = CPT_imu.roll;  //endl用于换行
					mcpt.pitch = CPT_imu.pitch ;  //endl用于换行
					mcpt.latitude = CPT_imu.latitude ;  //endl用于换行											
					mcpt.longitude = CPT_imu.longitude;  //endl用于换行
					mcpt.altitude = CPT_imu.altitude;  //endl用于换行
					mcpt.northVelocity = CPT_imu.northVelocity;  //endl用于换行
					mcpt.eastVelocity= CPT_imu.eastVelocity;  //endl用于换行
					mcpt.upVelocity= CPT_imu.upVelocity;  //endl用于换行
				cptListBuffer.push_back(mcpt);
				fronttime = CPT_imu.utime;
				if(CPT_imu.utime-fronttime!=20){
                    cout<<"parse time is wrong"<<endl;
                }
                fronttime = CPT_imu.utime;

		    }
			else if (ID == 1465)
			{
				//cout<<"1465"<<endl;
				len = (imu_strRecv[9] << 8) + imu_strRecv[8] + 32;
				if (imu_strRecv.size() < len) //数据区尚未接收完整，
				{
					break; //跳出接收函数后之后继续接收数据
				}
				float azimuthdev = 0;
				memcpy(&azimuthdev, imu_strRecv.data() + 28 + 116, 4);
				imu_strRecv.erase(imu_strRecv.begin(), imu_strRecv.begin() + len);
				CPT_imu.Z_Accuracy = azimuthdev;
                //ofs<<"i will write the imu data"<<endl;
                cout<<"1465 CPT_imu.Z_Accuracy is "<<(float)CPT_imu.Z_Accuracy<<endl;
			}
		}else {
            	//帧头长度不正确时，清除
				imu_strRecv.erase(imu_strRecv.begin(), imu_strRecv.end() ); //为啥begin()+1,这个1的实际长度多少
		}
		
	}
}
// char* gSerbufferIn;
bool ParseData::parse_data(std::vector<u_int8_t> strRecv)
{
	cout << "strRecv size:"<<strRecv.size()<<endl;
	imu_strRecv.insert(imu_strRecv.end(), strRecv.begin(), strRecv.end());
	while (	imu_strRecv.size() >=104) //至少包含帧头（2字节）、长度（1字节）、功能位（1字节）；根据设计不同而不同
	{
		//cout<<"104";
		// 2.1 查找数据头

		if (imu_strRecv[0] == 0xAA &&imu_strRecv[1] == 0x44) //传输数据有帧头，用于判断. 找到帧头  AA AA 0A
		{
			// byte[] longByte=new byte[2];
			//                                longByte[0] = imu_strRecv[4];
			//                                longByte[1] = imu_strRecv[5];
			ushort ID = 0;
			memcpy(&ID, imu_strRecv.data() + 4, 2);
			//                                longByte[0] = imu_strRecv[8];
			//                                longByte[1] = imu_strRecv[9];
			short len = 0;
      		ofstream ofs;
			if (ID == 508)
			{
				cout<<"508";
				//ofs.open(str1,ios::app);
				unsigned char length = (unsigned char)imu_strRecv[3];
				// cout<<"imu_strRecv[3]"<<(int)imu_strRecv[3]<<endl;
				len = (short)length + 16;
				// 				cout<<"length::"<<std::dec<<(int)length<<endl;
				// cout<<"len::"<<(int)len<<endl;
				    // cout<<"ID"<<ID<< " "<<len <<std::endl;
				if (imu_strRecv.size() < len) //数据区尚未接收完整，
				{
					break; //跳出接收函数后之后继续接收数据
				}
				//得到完整的数据，复制到ReceiveBytes中进行校验
				//dataBuffer.resize(len);
				//memcpy(&dataBuffer, imu_strRecv.data(), len);
				// dataBuffer = imu_strRecv.left(len);

				//imu_strRecv.erase(imu_strRecv.begin(), imu_strRecv.begin() + len);
				// imu_strRecv.remove(0, len);//从链表中移除接收到的校验失败的数据，
				//执行其他代码，对数据进行处理。
				//解析5 6， 7 8字节的经纬度.
				uint32_t week;
					cout<<"i will write the imu"<<endl;
					// cout<<"dataBuffer.data()"<<(dataBuffer.data())<<endl;
				memcpy(&CPT_imu, imu_strRecv.data() + 16, length);
				imu_strRecv.erase(imu_strRecv.begin(), imu_strRecv.begin() + len);
					// cout<<"i will write the imu back"<<endl;
				if (LCM_->good())
				{
				
					lcmtypes::gps_imu_info_t my_data;
					my_data.latitude = CPT_imu.latitude;
					my_data.longitude = CPT_imu.longitude;
					my_data.altitude = CPT_imu.altitude;
					my_data.velocityNorth = CPT_imu.northVelocity;
					my_data.velocityEast = CPT_imu.eastVelocity;
					my_data.velocityDown = CPT_imu.upVelocity;
					my_data.velocity =
						sqrt(my_data.velocityDown * my_data.velocityDown +
							 my_data.velocityEast * my_data.velocityEast +
							 my_data.velocityNorth * my_data.velocityNorth);
					my_data.yaw = CPT_imu.azimuth + gps_ini.yaw_c;
					if (CPT_imu.azimuth > 180)
						my_data.yaw = -360 + my_data.yaw;

					CPT_imu.azimuth = my_data.yaw;
					gps_ini.yaw = my_data.yaw * M_PI / 180;

					if (gps_ini.yaw <= M_PI && gps_ini.yaw >= -0.5 * M_PI)
					{
						gps_ini.yaw = -(-0.5 * M_PI + gps_ini.yaw);
					}
					else if (gps_ini.yaw >= -M_PI && gps_ini.yaw < -0.5 * M_PI)
					{
						gps_ini.yaw = -(1.5 * M_PI + gps_ini.yaw);
					}
					my_data.yaw = my_data.yaw * M_PI / 180;
					my_data.roll = CPT_imu.roll;
					my_data.pitch = CPT_imu.pitch;

					//                my_data.accelerationLateral=IMU.accY;
					//                my_data.accelerationLongitudinal=IMU.accX;

					gps_ini.alt_ini = my_data.altitude;
					gps_ini.Lat = my_data.latitude;
					gps_ini.Lon = my_data.longitude;
					gps_ini.lat_ini = my_data.latitude;
					gps_ini.lon_ini = my_data.longitude;
					gps_ini.vd = my_data.velocityDown;
					gps_ini.ve = my_data.velocityEast;
					gps_ini.vn = my_data.velocityNorth;
					gps_ini.velocity = my_data.velocity;
					//my_data.confidenceLevel = CPT_imu.Z_Accuracy;
					my_data.locationStatus = gps_ini.statu;
					my_data.satelliteNumber = gps_ini.StarNum;
					// my_data.utime= GetCurrentTimeInMilliSec();
					CPT_imu.utime = (long)((CPT_imu.week * 604800 + CPT_imu.seconds +
											315964800 - 18) *
										   1000000);
					my_data.utime = CPT_imu.utime;

					if (gps_ini.statu != 4)
					{
					}
					else
					{
						gps_ini.X_dev_x = 0;
						gps_ini.X_dev_y = 0;
					}
					my_data.latitudeEstimate = gps_ini.Lat;
					my_data.longitudeEstimate = gps_ini.Lon; 		
					
					if (LCM_->good())
							LCM_->publish("GPS_DATA", &my_data);
				}
			ofs.close();
			}
			else if (ID == 1465)
			{
				len = (imu_strRecv[9] << 8) + imu_strRecv[8] + 32;
				if (imu_strRecv.size() < len) //数据区尚未接收完整，
				{
					break; //跳出接收函数后之后继续接收数据
				}
				//得到完整的数据，复制到ReceiveBytes中进行校验
				//dataBuffer.resize(len + 28);
				//memcpy(&dataBuffer, imu_strRecv.data(), len + 28);
				// imu_strRecv.remove(0, len);//从链表中移除接收到的校验失败的数据，
				float azimuthdev = 0;
				memcpy(&azimuthdev, imu_strRecv.data() + 28 + 116, 4);
				imu_strRecv.erase(imu_strRecv.begin(), imu_strRecv.begin() + len);
				CPT_imu.Z_Accuracy = azimuthdev;//cout
			}
		}
		else //帧头不正确时，记得清除
		{
			imu_strRecv.erase(imu_strRecv.begin(), imu_strRecv.begin() + 1);
			// imu_strRecv.remove(0,1);//清除第一个字节，继续检测下一个。
		}
	}
}
std::vector<u_int8_t> ins_strRecv;
bool ParseData::parse_daoyuan_data(std::vector<u_int8_t> strRecv)
{

	struct timeval tv;
	gettimeofday(&tv, NULL);
	long lCurTime = tv.tv_sec /3600; //当前时间
	if (m_lCurTime != lCurTime)		  //每个小时重新生成要给保存数据的文件
	{
		m_fSave.close();
		m_strSaveFilePath = std::to_string(tv.tv_sec * 1000 + tv.tv_usec / 1000); //当前毫秒时间，构建保存数据名称
		m_fSave.open(m_strSaveFilePath);
		m_lCurTime = lCurTime;
		cout<<"time:"<<m_lCurTime<<"    Save infor in file :"<< m_strSaveFilePath<<endl;
	}

	// int i,j,k;
	std::vector<u_int8_t> dataBuffer;

	ins_strRecv.insert(ins_strRecv.end(), strRecv.begin(), strRecv.end());
	while (
		ins_strRecv.size() >=
		3) //至少包含帧头（2字节）、长度（1字节）、功能位（1字节）；根据设计不同而不同
	{
		// 1 查找数据头
		if (ins_strRecv[0] == 0xBD && ins_strRecv[1] == 0xDB &&
			ins_strRecv[2] == 0x0B) //传输数据有帧头，用于判断. 找到帧头  AA AA 0A
		{
			if (ins_strRecv.size() < 58) //数据区尚未接收完整，
			{
				break; //跳出接收函数后之后继续接收数据
			}
			//得到完整的数据，复制到ReceiveBytes中进行校验
			dataBuffer.resize(58);
			dataBuffer.assign(ins_strRecv.begin(), ins_strRecv.begin() + 58);
			// memcpy(&dataBuffer,ins_strRecv.data(),58);
			ins_strRecv.erase(ins_strRecv.begin(), ins_strRecv.begin() + 58);

			unsigned char x;
			x = 0;
			for (int i = 0; i < dataBuffer.size() - 1; i++)
				x ^= dataBuffer[i];
			unsigned char N = dataBuffer[dataBuffer.size() - 1];
			if (x != N)
			{
				// imu_strRecv.remove(0, 58);//从链表中移除接收到的校验失败的数据，
				continue; //继续执行while循环程序,
			}
			//执行其他代码，对数据进行处理。
			//解析5 6， 7 8字节的经纬度.
			memcpy(&DaoYuan_imu, dataBuffer.data(), dataBuffer.size());
			if (LCM_->good())
			{
				std::cout<<"LCM is good!!"<<std::endl;
				lcmtypes::gps_imu_info_t my_data;
				my_data.utime = GetCurrentTimeInMilliSec();
				my_data.latitude = ((double)DaoYuan_imu.lat) / 1e7f;
				my_data.longitude = ((double)DaoYuan_imu.lon) / 1e7f;
				my_data.altitude = ((float)DaoYuan_imu.altitude) / 1e3f;
				my_data.velocityNorth = ((float)DaoYuan_imu.V_north) * 100 / 32768;
				my_data.velocityEast = ((float)DaoYuan_imu.V_east) * 100 / 32768;
				my_data.velocityDown = ((float)DaoYuan_imu.V_ground) * 100 / 32768;
				my_data.velocity = sqrt(my_data.velocityDown * my_data.velocityDown +
										my_data.velocityEast * my_data.velocityEast +
										my_data.velocityNorth * my_data.velocityNorth);
				my_data.yaw = ((float)DaoYuan_imu.azimuth) * 360 / 32768;
				gps_ini.yaw = my_data.yaw * M_PI / 180;
				if (gps_ini.yaw <= M_PI && gps_ini.yaw >= -0.5 * M_PI)
				{
					gps_ini.yaw = -(-0.5 * M_PI + gps_ini.yaw);
				}
				else if (gps_ini.yaw >= -M_PI && gps_ini.yaw < -0.5 * M_PI)
				{
					gps_ini.yaw = -(1.5 * M_PI + gps_ini.yaw);
				}
				my_data.yaw = my_data.yaw * M_PI / 180;
				my_data.yaw = my_data.yaw + gps_ini.yaw_c * M_PI / 180;
				if (my_data.yaw > M_PI)
				{
					my_data.yaw = -2 * M_PI + my_data.yaw;
				}

				if (my_data.yaw < -M_PI)
				{
					my_data.yaw = 2 * M_PI + my_data.yaw;
				}
				my_yaw = my_data.yaw;
				my_data.roll = ((float)DaoYuan_imu.roll) * 360 / 32768;
				my_data.pitch = ((float)DaoYuan_imu.pitch) * 360 / 32768;
				vehicle_status.acclon_ins =
					9.81 * ((float)DaoYuan_imu.accX) * 12 / 32768;
				vehicle_status.acclat_ins =
					9.81 * ((float)DaoYuan_imu.accY) * 12 / 32768;
				my_data.accelerationLateral = vehicle_status.acclat_ins;
				my_data.accelerationLongitudinal = vehicle_status.acclon_ins;
				//                my_data.accelerationLateral=IMU.accY;
				//                my_data.accelerationLongitudinal=IMU.accX;
				my_data.yawRate = ((float)DaoYuan_imu.gyroscope_z) * 300 / 32768;
				float dy_time = ((float)DaoYuan_imu.time) / 4000;
				gps_ini.alt_ini = my_data.altitude;
				//                gps_ini.Lat=my_data.latitude;
				//                gps_ini.Lon=my_data.longitude;
				gps_ini.lat_ini = my_data.latitude;
				gps_ini.lon_ini = my_data.longitude;
				gps_ini.vd = my_data.velocityDown;
				gps_ini.ve = my_data.velocityEast;
				gps_ini.vn = my_data.velocityNorth;
				gps_ini.velocity = my_data.velocity;
				my_data.confidenceLevel = gps_ini.HDOP;
				my_data.locationStatus = gps_ini.statu;
				my_data.satelliteNumber = gps_ini.StarNum;
				//                if(gps_ini.latE>0)

				m_fSave << my_data.utime << " " << my_data.locationStatus << " " << my_data.latitude << " " << my_data.longitude << " " << my_data.altitude
						<< my_data.velocityNorth << " " << my_data.velocityEast << " " << my_data.velocityDown << " " << my_data.velocity
						<< my_data.accelerationLateral << " " << my_data.accelerationLongitudinal << my_data.yaw << " " << my_data.roll << " " << my_data.pitch
						<< " " << my_data.yawRate;

				// LCM_->publish("GPS_DATA", &my_data);
				if ((0x0F & DaoYuan_imu.state) == 0x0F)
				{
					DaoYuan_imu.status = true;
				}
				else
				{
					DaoYuan_imu.status = false;
				}
				if (DaoYuan_imu.data_type == 33)
				{
					DaoYuan_imu.wheelspeed = (bool)DaoYuan_imu.data1;
				}
				else if (DaoYuan_imu.data_type == 2)
				{
					DaoYuan_imu.yawPrecision = (float)(exp(DaoYuan_imu.data2 / 100));
				}
				else if (DaoYuan_imu.data_type == 22)
				{
					DaoYuan_imu.tempreture = (float)(DaoYuan_imu.data0 * 200 / 32768);
				}
				else if (DaoYuan_imu.data_type == 32)
				{
					DaoYuan_imu.GPSstute = DaoYuan_imu.data0;
					DaoYuan_imu.GPSNUM = DaoYuan_imu.data1;
					DaoYuan_imu.yawStute = DaoYuan_imu.data2;
				}
				// cout << gps_ini.lat_ini << "  " << gps_ini.lon_ini << "  "
				// 	 << gps_ini.velocity << "  " << gps_ini.yaw << "  "
				// 	 << DaoYuan_imu.GPSstute << "  " << DaoYuan_imu.yawStute << "  "
				// 	 << DaoYuan_imu.wheelspeed << "  " << DaoYuan_imu.status << endl;
				//cout << "Time:"<<my_data.utime<<"   RTK status:" << " " << my_data.locationStatus<<endl;
			}
		}
		else //帧头不正确时，记得清除
		{
			ins_strRecv.erase(
				ins_strRecv.begin(),
				ins_strRecv.begin() + 1); //清除第一个字节，继续检测下一个。
		}
	}
}
int ins_d_len = 109;
bool ParseData::parse_ins_d_data(std::vector<u_int8_t> strRecv)
{
	// int i,j,k;
	std::vector<u_int8_t> dataBuffer;

	ins_strRecv.insert(ins_strRecv.end(), strRecv.begin(), strRecv.end());
	while (
		ins_strRecv.size() >=
		4) //至少包含帧头（2字节）、长度（1字节）、功能位（1字节）；根据设计不同而不同
	{
		// 1 查找数据头
		if (ins_strRecv[0] == 0xAA && ins_strRecv[1] == 0x55 &&
			ins_strRecv[2] == 0x1 &&
			ins_strRecv[3] == 0x57) //传输数据有帧头，用于判断. 找到帧头  AA AA 0A
		{
			if (ins_strRecv.size() < ins_d_len) //数据区尚未接收完整，
			{
				break; //跳出接收函数后之后继续接收数据
			}
			//得到完整的数据，复制到ReceiveBytes中进行校验
			dataBuffer.resize(ins_d_len);
			dataBuffer.assign(ins_strRecv.begin(), ins_strRecv.begin() + ins_d_len);
			ins_strRecv.erase(ins_strRecv.begin(), ins_strRecv.begin() + ins_d_len);

			int32_t tmp_sum;
			tmp_sum = 0;
			for (int i = 2; i < dataBuffer.size() - 2; ++i)
			{
				tmp_sum += dataBuffer[i];
			}
			tmp_sum = tmp_sum & 0xffff;
			int N = dataBuffer[dataBuffer.size() - 1] * 256 +
					dataBuffer[dataBuffer.size() - 2];
			if (tmp_sum != N)
			{
				//从链表中移除接收到的校验失败的数据，
				ins_strRecv.erase(ins_strRecv.begin(), ins_strRecv.begin() + ins_d_len);
				//清除第一个字节，继续检测下一个。
				continue;
				//继续执行while循环程序,
			}
			//执行其他代码，对数据进行处理。
			//解析5 6， 7 8字节的经纬度.
			memcpy(&ins_d, dataBuffer.data(), dataBuffer.size());
			if (LCM_->good())
			{
				lcmtypes::gps_imu_info_t my_data;
				my_data.latitude = ((double)ins_d.lat) / 1e7f;
				my_data.longitude = ((double)ins_d.lon) / 1e7f;
				my_data.altitude = ((float)ins_d.altitude) * 0.03;
				my_data.velocityNorth = ((float)ins_d.V_north) * 0.01;
				my_data.velocityEast = ((float)ins_d.V_east) * 0.01;
				my_data.velocityDown = ((float)ins_d.V_tianxian) * (-1);
				my_data.velocity = sqrt(my_data.velocityDown * my_data.velocityDown +
										my_data.velocityEast * my_data.velocityEast +
										my_data.velocityNorth * my_data.velocityNorth);
				// my_data.yaw = ((float)ins_d.yaw) * 0.01 * M_PI / 180;
				my_data.yaw = ((float)ins_d.yaw) * 0.01;
				gps_ini.yaw = my_data.yaw * M_PI / 180;
				if (gps_ini.yaw <= M_PI && gps_ini.yaw >= -0.5 * M_PI)
				{
					gps_ini.yaw = -(-0.5 * M_PI + gps_ini.yaw);
				}
				else if (gps_ini.yaw >= -M_PI && gps_ini.yaw < -0.5 * M_PI)
				{
					gps_ini.yaw = -(1.5 * M_PI + gps_ini.yaw);
				}
				my_data.yaw = my_data.yaw * M_PI / 180;
				my_data.yaw = my_data.yaw + gps_ini.yaw_c * M_PI / 180;
				if (my_data.yaw > M_PI)
				{
					my_data.yaw = -2 * M_PI + my_data.yaw;
				}

				if (my_data.yaw < -M_PI)
				{
					my_data.yaw = 2 * M_PI + my_data.yaw;
				}

				my_data.roll = ((float)ins_d.roll) * 0.01 * M_PI / 180;
				my_data.pitch = ((float)ins_d.pitch) * 0.01 * M_PI / 180;
				my_data.accelerationLateral = ((float)ins_d.accX) * 9.8106 / 4000;
				my_data.accelerationLongitudinal = ((float)ins_d.accY) * 9.8106 / 4000;

				my_data.yawRate = 0;
				float dy_time = ((float)ins_d.time) / 1000;

				my_data.confidenceLevel = 1;
				my_data.locationStatus = ins_d.localization;
				my_data.satelliteNumber = (int)ins_d.StarNum;
				my_data.utime = GetCurrentTimeInMilliSec();
				LCM_->publish("GPS_DATA", &my_data);
				std::cout << setprecision(13) << "latitude"
						  << "\t" << my_data.latitude << "\t"
						  << "longitude"
						  << "\t" << my_data.longitude << '\t' << my_data.yaw << endl;
			}
		}
		else //帧头不正确时，记得清除
		{
			ins_strRecv.erase(
				ins_strRecv.begin(),
				ins_strRecv.begin() + 1); //清除第一个字节，继续检测下一个。
		}
	}
}

float Hex_To_Decimal(unsigned char *Byte, int num) //十六进制到浮点数
{
	return *((float *)Byte); //方法二
}
void FloatToByte(float floatNum,
				 unsigned char *byteArry) ////浮点数到十六进制转换2
{
	char *pchar = (char *)&floatNum;
	for (int i = 0; i < sizeof(float); i++)
	{
		*byteArry = *pchar;
		pchar++;
		byteArry++;
	}
}
